#ifndef OPTICAL_FLOW_TRACKER_H_
#define OPTICAL_FLOW_TRACKER_H_

#include <opencv2/opencv.hpp>

using namespace std;
using namespace cv;

/// Optical flow tracker and interface
class OpticalFlowTracker
{
public:
    OpticalFlowTracker(const Mat &img1_,
                       const Mat &img2_,
                       const vector<KeyPoint> &kp1_,
                       vector<KeyPoint> &kp2_,
                       vector<bool> &success_,
                       bool inverse_ = true, bool has_initial_ = false)
        : img1(img1_), img2(img2_), kp1(kp1_), kp2(kp2_),
          success(success_), inverse(inverse_), has_initial(has_initial_) {}

    void calculateOpticalFlow(const Range &range);

private:
    const Mat &img1;
    const Mat &img2;
    const vector<KeyPoint> &kp1;
    vector<KeyPoint> &kp2;
    vector<bool> &success;
    bool inverse = true;
    bool has_initial = false;
};

/**
 * get grayscale value from reference image (bi-linear interpolated)
 * @param img
 * @param x
 * @param y
 * @return the interpolated value of this pixel
 */
inline float GetGrayscale(const cv::Mat &img, float x, float y)
{
    // boundary check
    if (x < 0)
    {
        x = 0;
    }
    else if (x >= img.cols)
    {
        x = img.cols - 1;
    }

    if (y < 0)
    {
        y = 0;
    }
    else if (y >= img.rows)
    {
        y = img.rows - 1;
    }

    uchar *data = &img.data[int(y) * img.step + int(x)];

    float xx = x - floor(x);
    float yy = y - floor(y);

    // bi-linear interpolated
    // for 4 coordinates: (0,0), (0,1), (1,0), and (1,1)
    //    f(x,y) = f(0,0)(1-x)(1-y)
    //           + f(1,0)x(1-y) 
    //           + f(0,1)(1-x)y
    //           + f(1,1)xy
    return float((1 - xx) * (1 - yy) * data[0] +
                 xx * (1 - yy) * data[1] +
                 (1 - xx) * yy * data[img.step] +
                 xx * yy * data[img.step + 1]);
}

#endif
